#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <yarp/os/Network.h>
#include <yarp/os/Port.h>
#include <yarp/os/NetFloat64.h>

using namespace yarp::os;

typedef NetFloat64 TEST_TYPE;

/*
  Build a data type configured for maximum speed.  
  Not using convenience classes (Bottle/Vector/etc).
 */

class TestDatum : public Portable {
private:
  TEST_TYPE *fa;
public:
  TestDatum() {
    fa = new TEST_TYPE[10000000];
    if (fa==NULL) { printf("Memory allocation failure\n"); exit(1); }
  }

  virtual ~TestDatum() {
    delete fa;
    fa = NULL;
  }

  virtual bool write(ConnectionWriter& writer) {
    writer.appendExternalBlock((const char*) &fa[0],
			       sizeof(fa)/sizeof(TEST_TYPE));
    return true;
  }

  virtual bool read(ConnectionReader& reader) {
    return reader.expectBlock((const char*) &fa[0],
			      sizeof(fa)/sizeof(TEST_TYPE));
  }
  
  virtual void onCompletion() {
    // port has finished with this datum
  }
};


int reader_main() {
    Port p;
    if (!(p.open("..."))) {
      printf("Reader port failed to open - is YARP name server running?\n");
      return 1;
    }
    Network::connect("/float_array",p.getName().c_str(),"tcp");
    TestDatum datum;
    int ct = 0;
    while (true) {
      p.read(datum);
      ct++;
      if (ct%1000==0) {
	printf("Got datum #%d\n",ct);
      }
    }
    p.close();
    return 0;
}

int writer_main() {
    Port p;
    if (!(p.open("/float_array"))) {
      printf("Port failed to open - is YARP name server running?\n");
      return 1;
    }
    TestDatum datum;
    for (int i = 0; i < 10000000; ++i) {
      p.write(datum);
    }
    p.close();
    return 0;
}

int main(int argc, char *argv[]) {
  Network yarp;

  bool reader = false;
  bool writer = false;
  if (argc>1) {
    if (strcmp(argv[1],"read")==0) {
      reader = true;
    } else if (strcmp(argv[1],"write")==0) {
      writer = true;
    }
  }
  if (!(reader||writer)) {
    printf("Please supply 'read' or 'write' argument\n");
    return 1;
  }

  if (writer) {
    return writer_main();
  } else {
    return reader_main();
  }
}
